/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-22     Administrator       the first version
 */

#include "MotorDriver.h"
#include "DevManager.h"

#include <rtthread.h>
#include <cstring>
#include "ulog.h"



/**
简易通讯协议

第1字节    第2字节    第3字节    第4字节    第5字节    第6字节    第7字节    第8字节    总字节数    空闲字节数
1字节         1字节                                               0-5字节                                       1字节
                    操作模式                                        参数                                              校验
设备号        0速度点位     速度1字节   点位1字节   CRC8校验1字节               5   3
                    1速度相对位置 速度1字节   相对位置4字节   CRC8校验1字节   8   0
                    2速度  速度1字节   CRC8校验1字节                   4   4
                    3开关  开关1字节   CRC8校验1字节                   4   4
                    4延时开关   时间4字节               开关1字节   CRC8校验1字节   8   0
                    5模拟传感器  阈值4字节               CRC8校验1字节       5   3
                    6液面探测   次数1字节   下探深度4字节             CRC8校验1字节   8   0
                    7二值传感器  开关1字节   跳转步数1字节 CRC8校验1字节               5   3
                    8复位     CRC8校验1字节                       3   5
*/

TmcDriver tmc_driver0(0);
TmcDriver tmc_driver1(1);
TmcDriver tmc_driver2(2);
TmcDriver tmc_driver3(3);
TmcDriver tmc_driver4(4);
TmcDriver tmc_driver5(5);
TmcDriver tmc_driver6(6);

TmcDriver *tmc_driver[7] =
{
        &tmc_driver0,
        &tmc_driver1,
        &tmc_driver2,
        &tmc_driver3,
        &tmc_driver4,
        &tmc_driver5,
        &tmc_driver6,
};

/*定义一个函数指针,用于索引 */
void (*Motor_Driver_Task_Ptr[7]) (void *parameter) =
{
        Motor_Driver_0_Task,
        Motor_Driver_1_Task,
        Motor_Driver_2_Task,
        Motor_Driver_3_Task,
        Motor_Driver_4_Task,
        Motor_Driver_5_Task,
        Motor_Driver_6_Task,
};


/* rt-os 任务入口必须为静态函数 做一次封装 */
void Motor_Driver_0_Task(void *parameter){tmc_driver[0]->Task(parameter);}
void Motor_Driver_1_Task(void *parameter){tmc_driver[1]->Task(parameter);}
void Motor_Driver_2_Task(void *parameter){tmc_driver[2]->Task(parameter);}
void Motor_Driver_3_Task(void *parameter){tmc_driver[3]->Task(parameter);}
void Motor_Driver_4_Task(void *parameter){tmc_driver[4]->Task(parameter);}
void Motor_Driver_5_Task(void *parameter){tmc_driver[5]->Task(parameter);}
void Motor_Driver_6_Task(void *parameter){tmc_driver[6]->Task(parameter);}


// The constructor
TmcDriver::TmcDriver(rt_uint8_t _driver_num)
{
    driver_num = _driver_num;
}
// The destructor
TmcDriver::~TmcDriver(){
}


rt_uint32_t TmcDriver::init()
{
    return TMC5160_init(driver_num);
}


rt_uint32_t TmcDriver::getRunMode(void)
{
    return run_param.rampmode;
}

//rt_uint32_t TmcDriver::setRunMode(rt_uint8_t runMode)
//{
//    run_param.rampmode = runMode;
//}


rt_uint32_t TmcDriver::DriverParameterUpdate(int spdGrade)
{
    /**
     *  寄存器参数更新 通常包括
     *  运行电流参数 保持电流参数
     *  运动的加速度/减速度/速度/以及速度切换点
     */
    run_param.rampmode = POSITION;
    run_param.a1 = res_run_param[spdGrade].a1;
    run_param.amax = res_run_param[spdGrade].amax;
    run_param.d1 = res_run_param[spdGrade].d1;
    run_param.dmax = res_run_param[spdGrade].dmax;
    run_param.v1 = res_run_param[spdGrade].v1;
    run_param.vmax = res_run_param[spdGrade].vmax;
    run_param.vstart = res_run_param[spdGrade].vstart;
    run_param.vstop = res_run_param[spdGrade].vstop;

    //写入寄存器
    return 1;
}


rt_uint32_t TmcDriver::StartUp(void)
{
    TMC_START_PARA temp;
    temp.vmax = run_param.vmax;
    temp.rampmode = run_param.rampmode;
    temp.x_target = run_param.x_target;
    return start(driver_num,temp);
}
//Move to target position
rt_uint32_t TmcDriver::moveToPositon(rt_int32_t position)
{
    return moveTo(driver_num, position);
}

/*
 * Brief:
 * Name:
 * Parameter: velocity units are rps.
 * Author:
 * Date:
 */

rt_uint32_t TmcDriver::moveByVelocity(float32_t f_velocity)
{
    int32_t l_velocity = f_velocity * 200 * 256;
    return rotate(driver_num, l_velocity);
}

//Moving to the target position at a certain velocity
/*
 * Generally no acceleration is specified, so the acceleration
 * is assigned as 4 times the velocity, which can be modified
 * by macro definition.
 */
rt_uint32_t TmcDriver::moveToPosByVelocity(rt_int32_t position, float32_t velocity)
{
    return 1;
}

//Motor go-home function
/*
 * More advanced reset operation can eliminate mechanical clearance.
 */

rt_uint32_t TmcDriver::goHome(GoHome_Mode mode)
{

    return 1;
}
/*
 * Sometimes you need to specify reset speed.
 */
rt_uint32_t TmcDriver::goHomeByVelocity(float32_t velocity)
{
    return 1;
}


//User - defined complex functions, such as needle level detection
rt_uint8_t TmcDriver::positionCheck(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::positionReached(void)
{
    int32_t flag;
    GAP(TYPE_ID_POSITION_REACHED_FLAG,driver_num,&flag);
    if(flag == 1)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::velocityReached(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::stopReached(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::getCurPosition(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::goHomeMinimAccCal(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::SetTargetPos(rt_int32_t position)
{
    run_param.x_target = position;
    run_param.rampmode = POSITION;
    return true;
}





// Automatic calibration of motor
/*
 * Loop tasks:
 * Using state machine design in bare metal development,
 * Allocates priorities and time slices in the operating system.
 */
/**
 * Motor reset task.
 * @param parameter
 */

void TmcDriver::GoHomeTask(void *parameter)
{
    rt_int32_t position;
    /** 计算最小向正方向的偏移量  */
    position = goHomeMinimAccCal();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    //goHomeStage1();
    while(stopReached() == false)
    {
        rt_thread_mdelay(10);
    }
    position = getCurPosition();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    /** 释放完成信号 */
    rt_sem_release(task.motor_driver[driver_num].complete);
    return;
}


/**
 * 速度位移任务
 * @param spdGrade speed grade
 * @param mode     motion mode
 * @param para1
 * @param para2
 */
void TmcDriver::PosSpdTask(int spdGrade,int mode,uint32_t pos)
{
    /** 更新速度参数寄存器     */
    DriverParameterUpdate(spdGrade);
    /** 目标位置更新 */
    if (mode == POSITION)
    {
        run_param.x_target = pos;
    }
    /** 启动电机 */
    StartUp();
    while(1)
    {
        switch(mode)
        {
        case POSITION:
            if(positionReached() == true)
            {
                rt_thread_mdelay(10);
                rt_sem_release(task.motor_driver[driver_num].complete);
                return;
            }
            rt_thread_mdelay(100);
            break;
        case VELOCITY_CW:
        case VELOCITY_ACW:
            if(velocityReached() == true)
            {
                rt_sem_release(task.motor_driver[driver_num].complete);
                return;
            }
            rt_thread_mdelay(100);
        default:break;
        }
    }
}

/**
 * Liquid level detection mission.
 * @param parameter
 */
void TmcDriver::LiqDetTask(void *parameter)
{
    rt_int32_t position;
    position = goHomeMinimAccCal();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    //goHomeStage1();
    while(stopReached() == false)
    {
        rt_thread_mdelay(10);
    }
    position = getCurPosition();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    /** 释放完成信号 */
    rt_sem_release(task.motor_driver[driver_num].complete);
    return;
}

uint32_t position_circular_optimize(uint8_t devNo,uint32_t current_pos,uint32_t target_pos)
{
    uint32_t new_target_pos = 0;
    uint32_t relative_position = 0;
    uint32_t u_step_per_revolution;
    uint32_t half_u_step_per_revolution;
    relative_position = target_pos - current_pos;
    u_step_per_revolution = QueryOfDevUStepPerRevolution(devNo);
    half_u_step_per_revolution = (uint32_t) u_step_per_revolution / 2;
    while(relative_position > half_u_step_per_revolution)
    {
        relative_position -= u_step_per_revolution;
    }
    while(relative_position > half_u_step_per_revolution)
    {
        relative_position += u_step_per_revolution;
    }
    new_target_pos  = current_pos + relative_position;
    return new_target_pos;
}


/**
 * 电机运行任务执行过程描述
 * 进入条件为收到任务消息
 * 结束后释放一个电机完成信号
 * @param parameter
 */
void TmcDriver::Task(void *parameter)
{

    uint8_t Task[8] ={0};
    uint8_t devNo = 0,mode = 0,spdGrade = 0,position_dot = 0;
    uint32_t current_pos = 0,target_pos = 0,new_target_pos = 0;
    /** 驱动器参数初始化  */
    while(1)
    {
        /** 接收任务消息 */
        rt_mq_recv(task.motor_driver[driver_num].task_msg_queue, Task, 8, RT_WAITING_FOREVER);
        /** 解析任务动作 */
        devNo = Task[0];
        mode = Task[1];
        switch (mode)
        {
        case 0:/**复位模式*/
            GoHomeTask(NULL);
            break;
        case 1:/**速度点位模式*/
            spdGrade = Task[2];
            position_dot = Task[3];
            target_pos = position_model_calculator(devNo,position_dot);
            if(QueryOfDevCircleOptimize(devNo) == true)
            {
                new_target_pos = position_circular_optimize(devNo,current_pos,target_pos);
                PosSpdTask(spdGrade,mode,new_target_pos);
            }
            else
            {
                PosSpdTask(spdGrade,mode,target_pos);
            }
            break;
        case 2:/**速度点位模式*/
            spdGrade = Task[2];
            Byte_to_Uint32t(&target_pos, &Task[3]);
            PosSpdTask(spdGrade,mode,target_pos);
            break;
        case 3:
            LiqDetTask(NULL);
            break;
        default:
            LOG_E("motor task data error.");
            break;
        }
        /** 执行结束释放电机完成信号量 */
        rt_sem_release(task.motor_driver[driver_num].complete);
    }
}

